#include <all_lib.h>
#pragma hdrstop

#include "gh615.h"
#include "gh_utils.h"

/**********************************************************************
 DBG
**********************************************************************/
#if 0
  #if defined(__HCONSOLE__)
    #include "con_tst.inc"

    #define  Log( v )           do{ printf v; printf("\n"); }while(0)
    #define  DMP( nm, b, l )    do{ printf( "%s\n", nm ); PDump( b,l,2 ); }while(0)
  #else
    #define  Log( v )           INProc::Say v
    #define  DMP( nm, b, l )
  #endif
#else
  #define  Log( v )
  #define  DMP( nm, b, l )
#endif

/**********************************************************************
 LOCALS
**********************************************************************/
LOCALSTRUCT( GH615_SaveDevice )
  GH615& gh;
  BOOL isSelf,
       isOK;

  GH615_SaveDevice( GH615& g ) : gh(g) {
    isSelf = FALSE;
    if ( gh.port.isOK() )
      isOK = TRUE;
     else
      isOK = isSelf = gh.OpenDevice( -1 );
  }
  ~GH615_SaveDevice() {
    if ( isSelf )
      gh.CloseDevice();
  }

  operator int() { return isOK; }
};

inline DWORD MkTimeout( int bytes ) { return Max( 10, GHD_SPEED * bytes / 10 / 1000 ); }
inline WORD  RotateWORD( WORD v )   { return MK_WORD( LO_BYTE(v), HI_BYTE(v) ); }
inline DWORD RotateDWORD( DWORD v ) { return MK_DWORD( RotateWORD( LO_WORD(v) ), RotateWORD( HI_WORD(v) ) ); }

static BYTE GH615_CalcChecksum( BYTE start, LPCBYTE b, DWORD maxlen )
  {
     for( DWORD n = 0; n < maxlen; b++,n++ )
       start ^= *b;
 return start;
}

static DWORD GH615_Checksum( LPCBYTE b, BYTE id, DWORD maxlen )
  {
     if ( maxlen == 0 ) {
       SetError( "Device is not respond. Make shure it turned ON and set to \"Cfg\"->\"Data link to PC\" mode." );
       return INFINITE;
     }

     if ( maxlen < 4 /*id+len+checksum*/ ) {
       SetError( "Insufissient reply received" );
       return INFINITE;
     }

    //Check id
     if ( id )
       if ( b[0] != id ) {
         SetError( "Invalid info data header ID" );
         return INFINITE;
       }

    //Get length
     DWORD off = MK_WORD( b[1], b[2] ) + 3;
     if ( maxlen < off ) {
       SetError( "Partial info received %d instead %d", maxlen, off );
       return INFINITE;
     }

    //Calc and check checksum
     BYTE c = GH615_CalcChecksum( 0, b+1, off-1 );
     if ( c != b[off] ) {
       SetError( "Invalid data checksum %0xX instead of %02X", c, b[off] );
       return INFINITE;
     }
 return off - 3;
}

static void CopyText( char *d, char *s, size_t ss )
  {
     for( size_t n = 0; n < ss && *s && !isspace(*s); s++,d++,n++ )
       *d = *s;
     *d = 0;
}

/**********************************************************************
   GH615
**********************************************************************/
GH615::GH615( int Port, int CharReadTimeout /*= 100*/ )
    : nPort( Port ),
      Waypoints( 10, 10 ),
      Tracks( 10, 10 ),
      TrackPoints( 100, 100 )
  {
    OnProgress = NULL;
    nReply     = 0;

    SetConnectParams( GHD_SPEED, GHD_BITS, GHD_PARITY, GHD_STOP, GHD_FLOW,
                      CharReadTimeout );
}

DWORD   GH615::ReplyLen( void )  const { return nReply; }
LPCBYTE GH615::ReplyData( void ) const { return Reply; }
BOOL    GH615::Opened( void )    const { return port.isOK(); }

BOOL GH615::SetConnectParams( int bps,     // CBR_xx
                              int bits,    // 5,6,7,8
                              int parity,  // xxPARITY
                              int sbits,   // xxSTOPBIT
                              int flow,    // FLOW_xx
                              int CharReadTimeout /*= 100*/ )
  {
    port.RdTimeout = CharReadTimeout;
 return port.Setup( bps, bits, parity, sbits, flow );
}

BOOL GH615::OpenDevice( int nport )
  {
     if ( port.isOK() )
       return TRUE;

     if ( nport != -1 ) nPort = nport;
     Log(( "Opening port [%d]...", nPort ));
     if ( !port.Open( nPort ) ) {
       Log(( "!Open port" ));
       return FALSE;
     }
 return TRUE;
}

BOOL GH615::OpenDevice( CONSTSTR pnm )
  {
     if ( port.isOK() )
       return TRUE;

     if ( !pnm ) pnm = HPort::PortNum2Str( nPort );
     Log(( "Opening port [%d]...", pnm ));
     if ( !port.Open( pnm ) ) {
       Log(( "!Open port" ));
       return FALSE;
     }
 return TRUE;
}

void GH615::CloseDevice( void )
  {
     if ( !port.isOK() )
       return;

     Log(( "Close port" ));
     port.Close();
}

BOOL GH615::ReadReply( DWORD w )
  {
     if ( !port.isOK() )
       return FALSE;

     if ( nReply >= sizeof(Reply) ) {
       FIO_SETERRORN( ERROR_INSUFFICIENT_BUFFER );
       return FALSE;
     }

     Log(( "Waiting for responce [%d] ms...", w ));

     DWORD cn = port.Read( Reply+nReply, sizeof(Reply)-nReply, w );
     Log(( "Reply has %d bytes", cn ));
     DMP( "RPL:", Reply+nReply, cn );
     nReply += cn;

 return TRUE;
}

BOOL GH615::ReadReplyTm( DWORD tmOut )
  {  DWORD b = GetTickCount(),
           e;

     do{
       if ( !ReadReply( INFINITE ) ) {
         SetError( "Error read device reply" );
         return FALSE;
       }
       if ( ReplyLen() )
         return TRUE;

       Sleep( 200 );
       if ( tmOut != INFINITE ) {
         e = GetTickCount();
         if ( e-b > tmOut ) {
           FIO_SETERRORN( ERROR_TIMEOUT );
           return FALSE;
         }
       }
     }while( 1 );
}

BOOL GH615::ExecuteCmd( LPCVOID cmd, DWORD len, DWORD KnownRecvTimeout /*= INFINITE*/ )
  {  GH615_SaveDevice _save(*this);
     if ( !_save ) return FALSE;

     Log(( "Sending [%d] bytes", len ));
     DMP( "CMD:", cmd, len );
     if ( port.Write( cmd, len, INFINITE ) != len ) {
       Log(( "!send cmd" ));
       return FALSE;
     }

     nReply = 0;
     if ( KnownRecvTimeout )
       ReadReply( KnownRecvTimeout );

 return TRUE;
}

/**********************************************************************
 CMDS
**********************************************************************/
  static BYTE cmd_FirmwareEnd[]            = { 0x11, 0x00, 0x01, 0x00, 0x01 };
  static BYTE cmd_FirmwareStart[]          = { 0x11, 0x00, 0x02, 0x00, 0x00, 0x01 };
//static BYTE cmd_UpdateWp[]               = { 0x02, 0x00, LEN,  0x76, CNh, CNl, [<WP>, ...] <CHKS> };
  static BYTE cmd_GetWaypoints[]           = { 0x02, 0x00, 0x01, 0x77, 0x76 };
  static BYTE cmd_GetTrackList[]           = { 0x02, 0x00, 0x01, 0x78, 0x79 };
  static BYTE cmd_DelAllTracks[]           = { 0x02, 0x00, 0x03, 0x79, 0x00, 0x64, 0x1E };
//static BYTE cmd_DelTrack[]               = { 0x02, 0x00, 0x03, 0x79, CNh, CNl, [<IDh>, <IDl>, ...] <CHKS> };
//static BYTE cmd_TrackPoints[]            = { 0x02, 0x00, LEN,  0x80, CNh, CNl, [<IDh>, <IDl>, ...] <CHKS> };
  static BYTE cmd_GetNextTrackSegment[]    = { 0x02, 0x00, 0x01, 0x81, 0x80 };
  static BYTE cmd_GetCurrentTrackSegment[] = { 0x02, 0x00, 0x01, 0x82, 0x83 };
  static BYTE cmd_UNK1[]                   = { 0x02, 0x00, 0x01, 0x83, 0x82 }; //Respond with dump started: 0x83 of size 0x4B0+4
  static BYTE cmd_GetDevInfo[]             = { 0x02, 0x00, 0x01, 0x85, 0x84 };
//static BYTE cmd_SendTrack[]              = { 0x02, 0x00, 0x01, 0x90, 0x91, ... };
//static BYTE cmd_RetrySendTrackData[]     = { 0x02, 0x00, 0x01, 0x91, 0x90 }; //Got: 90 00 00 00 92 00 02 00 88 8A
  static BYTE cmd_UNK2[]                   = { 0x02, 0x00, 0x01, 0xA0, 0xA1 }; //Respond with dump started: 0xA0 of size 0x400+4

/**********************************************************************
 TST
**********************************************************************/
BOOL GH615::TST( void )
  {  GH615_SaveDevice _save(*this);
     if ( !_save ) return FALSE;

  static BYTE cmd[]  = { 0x01, 0x00, 0x01, 0, 0 };

     for( BYTE n = 0x1; n < 0xFF; n++ ) {
       cmd[3] = n;
       cmd[4] = 1 ^ n;

       if ( !ExecuteCmd( cmd, sizeof(cmd), 0 ) )
         return FALSE;
       if ( !ReadReplyTm( 500 ) )
         continue;

       n++;
     }

 return TRUE;
}

/**********************************************************************
 INFO
   Res [90]:
   0000: 85 00 56 47 48 2D 36 31 35 42 00 00 00 00 00 00 ..VGH-615B......
   0010: 00 00 00 00 00 00 00 00 00 6B 01 08 46 2D 30 54 .........k..F-0T
   0020: 48 2D 52 55 2D 30 37 30 36 30 37 31 00 55 53 45 H-RU-0706071.USE
   0030: 52 20 4E 41 4D 45 20 00 00 1A 00 96 00 44 00 46 R NAME ......D.F
   0040: 00 B2 02 07 00 07 BE 01 01 FF FF FF FF FF FF FF ................
   0050: FF FF FF FF FF FF FF FF FF 87                   ..........
 **********************************************************************/
#define GHD_INFO_ID    0x85

#include <Global/pack1.h>
STRUCT( GH615_DeviceInfo_LL )  //HEX
  char DeviceName[ 0x16 ];  //+3
  BYTE Version;             //+19
  BYTE Unk1;                //+1A
  BYTE Unk2;                //+1B
  char Firmware[ 0x11 ];    //+1C
  char User[ 0x0B ];        //+2C
  BYTE Sex;
  BYTE Age;                 //+39
  WORD WeightPounds;        //+3B
  WORD WeightKilo;          //+3D
  WORD HeightInch;          //+3F
  WORD HeightCm;            //+41
  BYTE WpCount;             //+42
  BYTE TrCount;             //+43
  BYTE Unk3;                //+44
  WORD bYear;               //+45   (0..)
  BYTE bMon;                //+47   (0..11)
  BYTE bDay;                //+49   (1..)
};
#include <Global/pop.h>

BOOL GH615::QueryInfo( void )
  {
#if 0
static BYTE rpl[] = {
 /*0000:*/0x85,0x00,0x56,0x47, 0x48,0x2D,0x36,0x31, 0x35,0x42,0x00,0x00, 0x00,0x00,0x00,0x00, //..VGH-615B......
 /*0010:*/0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00, 0x00,0x6B,0x01,0x08, 0x46,0x2D,0x30,0x54, //.........k..F-0T
 /*0020:*/0x48,0x2D,0x52,0x55, 0x2D,0x30,0x37,0x30, 0x36,0x30,0x37,0x31, 0x00,0x55,0x53,0x45, //H-RU-0706071.USE
 /*0030:*/0x52,0x20,0x4E,0x41, 0x4D,0x45,0x20,0x00, 0x00,0x1A,0x00,0x96, 0x00,0x44,0x00,0x46, //R NAME ......D.F
 /*0040:*/0x00,0xB2,0x02,0x07, 0x00,0x07,0xBE,0x01, 0x01,0xFF,0xFF,0xFF, 0xFF,0xFF,0xFF,0xFF, //................
 /*0050:*/0xFF,0xFF,0xFF,0xFF, 0xFF,0xFF,0xFF,0xFF, 0xFF,0x87                                 //..........
};
     nReply = sizeof(rpl);
     MemMove( Reply, rpl, nReply );
#else
     if ( !ExecuteCmd( cmd_GetDevInfo, sizeof(cmd_GetDevInfo), INFINITE ) )
       return FALSE;
#endif
     if ( GH615_Checksum( ReplyData(), GHD_INFO_ID, ReplyLen() ) == INFINITE )
       return FALSE;

     PGH615_DeviceInfo_LL lp = (PGH615_DeviceInfo_LL)(ReplyData()+3);

     CopyText( DeviceInfo.DeviceName, lp->DeviceName, sizeof(lp->DeviceName) );
     CopyText( DeviceInfo.Firmware,   lp->Firmware,   sizeof(lp->Firmware) );
     CopyText( DeviceInfo.User,       lp->User,       sizeof(lp->User) );

     DeviceInfo.Version      = lp->Version;
     DeviceInfo.SexMale      = lp->Sex == 0;
     DeviceInfo.Age          = lp->Age;
     DeviceInfo.WeightPounds = RotateWORD( lp->WeightPounds );;
     DeviceInfo.WeightKilo   = RotateWORD( lp->WeightKilo );
     DeviceInfo.HeightInch   = RotateWORD( lp->HeightInch );
     DeviceInfo.HeightCm     = RotateWORD( lp->HeightCm );
     DeviceInfo.WpCount      = lp->WpCount;
     DeviceInfo.TrCount      = lp->TrCount;
     DeviceInfo.Birth.year   = RotateWORD( lp->bYear );
     DeviceInfo.Birth.mon    = lp->bMon + 1;
     DeviceInfo.Birth.mday   = lp->bDay;

 return TRUE;
}

/**********************************************************************
 GET TRACK LIST
**********************************************************************/
#define GHD_TRACKLIST_ID    0x78

#include <Global/pack1.h>
LOCALSTRUCT( GH615_Track_LL )
  BYTE   Year,      //+00
         Mon,
         Day,
         Hour,
         Min,
         Sec;
  DWORD  Duration;  //+06
  DWORD  Distance;  //+10
  WORD   Calories;  //+14
  WORD   TopSpeed;  //+16
  DWORD  nPoints;   //+18
  WORD   Id;        //+20
};                  // 21
#include <Global/pop.h>

static void ParceTrackInfo( PGH615_Track p, PGH615_Track_LL lp )
  {
       p->Id        = RotateWORD( lp->Id );
       p->Time.year = 2000 + lp->Year;
       p->Time.mon  = lp->Mon;
       p->Time.mday = lp->Day;
       p->Time.hour = lp->Hour;
       p->Time.min  = lp->Min;
       p->Time.sec  = lp->Sec;

       lp->Duration = RotateDWORD( lp->Duration ) / 10;
       p->Duration.hour = lp->Duration / 3600;
       p->Duration.min  = (lp->Duration % 3600 ) / 60;
       p->Duration.sec  = lp->Duration % 60;

       p->Distance  = RotateDWORD( lp->Distance ) / 1000.;
       p->Calories  = RotateWORD( lp->Calories );
       p->TopSpeed  = RotateWORD( lp->TopSpeed ) / 100.;
       p->nPoints   = RotateDWORD( lp->nPoints );
}

BOOL GH615::QueryTracks( void )
  {
#if 0
/*
 id |         date        | distance | calories | topspeed | trackpoints
 00 | 2008-01-03 01:34:03 | 00000000 | 00000000 | 00000000 | 00000002
 01 | 2008-01-03 01:34:18 | 00003473 | 00000157 | 00000959 | 00002447
 10 | 2008-01-03 18:15:27 | 00000553 | 00000017 | 00001380 | 00000208
 11 | 2008-01-04 00:19:16 | 00000477 | 00000019 | 00001030 | 00000222
 12 | 2008-01-05 14:31:59 | 00007977 | 00000324 | 00005410 | 00003834
 27 | 2008-01-05 18:47:58 | 00030440 | 00000797 | 00008134 | 00006666
 52 | 2008-01-05 23:45:31 | 00005376 | 00000000 | 00001078 | 00005206
 72 | 2008-01-06 23:02:51 | 00004172 | 00000000 | 00005052 | 00001667
 79 | 2008-01-07 17:46:22 | 00008301 | 00000245 | 00001678 | 00005667
100 | 2008-01-08 00:14:09 | 00004725 | 00000105 | 00002904 | 00002157
110 | 2008-01-08 16:48:35 | 00035531 | 00001350 | 00007451 | 00009703
*/
static BYTE rpl[] = {
 /*000*/ 0x78,0x01,0x08,0x08,  0x01,0x03,0x01,0x22,  0x03,0x00,0x00,0x00,  0x07,0x00,0x00,0x00,   //x......"........
 /*010*/ 0x00,0x00,0x00,0x00,  0x00,0x00,0x00,0x00,  0x02,0x00,0x00,0x08,  0x01,0x03,0x01,0x22,   //..............."
 /*020*/ 0x12,0x00,0x00,0x8F,  0x37,0x00,0x00,0x0D,  0x91,0x00,0x9D,0x03,  0xBF,0x00,0x00,0x09,   //....7...........
 /*030*/ 0x8F,0x00,0x01,0x08,  0x01,0x03,0x12,0x0F,  0x1B,0x00,0x00,0x08,  0x3B,0x00,0x00,0x02,   //............;...
 /*040*/ 0x29,0x00,0x11,0x05,  0x64,0x00,0x00,0x00,  0xD0,0x00,0x0A,0x08,  0x01,0x04,0x00,0x13,   //)...d...........
 /*050*/ 0x10,0x00,0x00,0x09,  0x0F,0x00,0x00,0x01,  0xDD,0x00,0x13,0x04,  0x06,0x00,0x00,0x00,   //................
 /*060*/ 0xDE,0x00,0x0B,0x08,  0x01,0x05,0x0E,0x1F,  0x3B,0x00,0x00,0xA3,  0xBC,0x00,0x00,0x1F,   //........;.......
 /*070*/ 0x29,0x01,0x44,0x15,  0x22,0x00,0x00,0x0E,  0xFA,0x00,0x0C,0x08,  0x01,0x05,0x12,0x2F,   //).D."........../
 /*080*/ 0x3A,0x00,0x01,0x19,  0x6C,0x00,0x00,0x76,  0xE8,0x03,0x1D,0x1F,  0xC6,0x00,0x00,0x1A,   //:...l..v........
 /*090*/ 0x0A,0x00,0x1B,0x08,  0x01,0x05,0x17,0x2D,  0x1F,0x00,0x01,0xB6,  0xE2,0x00,0x00,0x15,   //.......-........
 /*0A0*/ 0x00,0x00,0x00,0x04,  0x36,0x00,0x00,0x14,  0x56,0x00,0x34,0x08,  0x01,0x06,0x17,0x02,   //....6...V.4.....
 /*0B0*/ 0x33,0x00,0x00,0x76,  0x97,0x00,0x00,0x10,  0x4C,0x00,0x00,0x13,  0xBC,0x00,0x00,0x06,   //3..v....L.......
 /*0C0*/ 0x83,0x00,0x48,0x08,  0x01,0x07,0x11,0x2E,  0x16,0x00,0x02,0x39,  0x26,0x00,0x00,0x20,   //..H........9&..
 /*0D0*/ 0x6D,0x00,0xF5,0x06,  0x8E,0x00,0x00,0x16,  0x23,0x00,0x4F,0x08,  0x01,0x08,0x00,0x0E,   //m.......#.O.....
 /*0E0*/ 0x09,0x00,0x01,0x8D,  0x66,0x00,0x00,0x12,  0x75,0x00,0x69,0x0B,  0x58,0x00,0x00,0x08,   //....f...u.i.X...
 /*0F0*/ 0x6D,0x00,0x64,0x08,  0x01,0x08,0x10,0x30,  0x23,0x00,0x01,0x99,  0xB7,0x00,0x00,0x8A,   //m.d....0#.......
 /*100*/ 0xCB,0x05,0x46,0x1D,  0x1B,0x00,0x00,0x25,  0xE7,0x00,0x6E,0x07                          //..F....%..n.
};
     nReply = sizeof(rpl);
     MemMove( Reply, rpl, nReply );
#else
     GH615_SaveDevice _save(*this);
     if ( !_save ) return FALSE;

     if ( !ExecuteCmd( cmd_GetTrackList, sizeof(cmd_GetTrackList), 0 ) )
       return FALSE;
     if ( !ReadReplyTm(GHD_TRACKLIST_TMOUT) )
       return FALSE;
#endif

     Tracks.DeleteAll();

     DWORD len;
     if ( (len=GH615_Checksum( ReplyData(), GHD_TRACKLIST_ID, ReplyLen() )) == INFINITE )
       return FALSE;

     if ( (len%sizeof(GH615_Track_LL)) != 0 ) {
       SetError( "Invalid TRl structure" );
       return FALSE;
     }

     PGH615_Track_LL lp = (PGH615_Track_LL)(ReplyData()+3);

     for( len /= sizeof(GH615_Track_LL); len > 0; len--, lp++ ) {
       GH615_Track p;
       ParceTrackInfo( &p, lp );
       Tracks.Add( p );
     }

 return TRUE;
}
/**********************************************************************
 DEL SINGLE TRACK DATA
**********************************************************************/
#define GHD_DELALLTRACK_ID    0x79

BOOL GH615::DelTrackData( int id )
  {
                           //                    Cmd   Cn          Id   Ch
static BYTE cmd_DelTrack[] = { 0x02, 0x00, 0x05, 0x79, 0x00, 0x01, 0,0, 0 };

      cmd_DelTrack[6] = HI_BYTE( id );
      cmd_DelTrack[7] = LO_BYTE( id );
      cmd_DelTrack[8] = GH615_CalcChecksum( 0, cmd_DelTrack+2, 6 );

      GH615_SaveDevice _save(*this);
      if ( !_save) return FALSE;

      if ( !ExecuteCmd( cmd_DelTrack, sizeof(cmd_DelTrack), 0 ) )
        return FALSE;
      if ( !ReadReplyTm( GHD_DELTRACK_TMOUT ) )
        return FALSE;

     if ( ReplyLen() < 1 + 2 + 1 ||
          ReplyData()[0] != GHD_DELALLTRACK_ID ) {
       SetError( "Invalid DelTR responce" );
       return FALSE;
     }

 return TRUE;
}
/**********************************************************************
 DEL ALL TRACK DATA
**********************************************************************/
BOOL GH615::DelAllTracks( DWORD tmOut )
  {  GH615_SaveDevice _save(*this);
     if ( !_save) return FALSE;

     if ( !ExecuteCmd( cmd_DelAllTracks, sizeof(cmd_DelAllTracks), 0 ) )
       return FALSE;

     if ( !ReadReplyTm( tmOut ) )
       return FALSE;

     if ( ReplyLen() < 1 + 2 + 1 ||
          ReplyData()[0] != GHD_DELALLTRACK_ID ) {
       SetError( "Invalid DelTR responce" );
       return FALSE;
     }

 return TRUE;
}

/**********************************************************************
 GET TRACK CONTENTS
**********************************************************************/
BOOL GH615::QueryTrack( int id )
  {  GH615_SaveDevice _save(*this);
     if ( !_save ) return FALSE;

 return DoQueryTrack( id );
}

#include <Global/pack1.h>
LOCALSTRUCT( qt_LL )
  WORD id;
  BYTE len;
  BYTE cmd;
  WORD numberOfTracks;
  WORD trackId;
  BYTE checksum;
};

LOCALSTRUCTBASE( GH615_Track_Segment_LL, public GH615_Track_LL )
  BYTE from;
  BYTE to;
};

LOCALSTRUCT( GH615_TrackPoint_LL )
  DWORD Lat;
  DWORD Lon;
  WORD  Alt;
  WORD  Speed;
  BYTE  HeartRate;
  WORD  Interval;
};
#include <Global/pop.h>

LOCALSTRUCTBASE( GH615_TrackSegment, public GH615_Track )
  int from;
  int to;
};

static void ParceTrackSegmentInfo( PGH615_TrackSegment p, PGH615_Track_Segment_LL lp )
  {
    ParceTrackInfo( p, lp );
    p->from = lp->from;
    p->to   = lp->to;
}

BOOL GH615::DoQueryTrack( int id )
  {
#if 0
TRACK DATA
  -->   02 00 05 80 00 01 00 00 84      (for track ID==00)
  <--   0x80,0x00,0x38,0x08,  0x01,0x03,0x01,0x22,  0x03,0x00,0x00,0x00,  0x07,0x00,0x00,0x00,  //..8...."........
 /*10*/ 0x00,0x00,0x00,0x00,  0x00,0x00,0x00,0x00,  0x02,0x00,0x00,0x00,  0x01,0x03,0x63,0x14,  //..............c.
 /*20*/ 0xA1,0x03,0x9C,0xA2,  0x59,0x00,0xF5,0x00,  0x68,0x00,0x00,0x00,  0x03,0x63,0x14,0xA1,  //....Y...h....c..
 /*30*/ 0x03,0x9C,0xA2,0x59,  0x00,0xF5,0x00,0x68,  0x00,0x00,0x07,0x11                         //...Y...h....
};

EOT
  --> 02 00 01 81 80    (next track)
  <-- 8A 00 00 00
#endif

     GH615_TrackSegment track;
     PGH615_Track_Segment_LL lt;
     LPCBYTE            b;
     DWORD              len, n;
     int                nRetry = 0,
                        nWait;
     time_t             tTm;
     int                tmAdd;

     TrackPoints.DeleteAll();

     qt_LL q;
     q.id             = 0x0002;
     q.len            = 0x05;
     q.cmd            = 0x80;
     q.numberOfTracks = RotateWORD( 1 );
     q.trackId        = RotateWORD( id );
     q.checksum       = GH615_CalcChecksum( 0, (LPBYTE)(&q.len), 6 );
     if ( !ExecuteCmd( &q, sizeof(q), 0 ) )
       return FALSE;

     while( 1 ) {
     //Get reply
       nWait = 0;
       do{
         if ( !ReadReply( INFINITE ) ) {
           SetError( "Error read device reply" );
           return FALSE;
         }
         if ( ReplyLen() )
           break;

         nWait++;

         if ( nWait == 2 ) {
           SetError( "Device is not respond. Make shure it turned ON and set to \"Cfg\"->\"Data link to PC\" mode." );
           return FALSE;
         }
         Sleep(200);
       }while(1);

       //Check reply
       do{
         if ( (len=GH615_Checksum( ReplyData(), 0, ReplyLen() )) != INFINITE )
           break;

       //Retry reply
         Log(( "Error reading reply, retrying..." ));
         nRetry++;
         if ( nRetry >= 3 ) {
           SetError( "Max retry of %d in getting reply reached", nRetry );
           return FALSE;
         }

       //Reget segment
         if ( !ExecuteCmd( &cmd_GetCurrentTrackSegment, sizeof(cmd_GetCurrentTrackSegment), INFINITE ) )
           return FALSE;
       }while( 1 ); //Read reply

       b = ReplyData();

     //Analyze reply
       if ( *b == 0x80 ) {
         Log(( "Track segment" ));
         if ( len < sizeof(GH615_Track_Segment_LL) ) {
           SetError( "Small track header length (%d)", len );
           return FALSE;
         }

         lt = (PGH615_Track_Segment_LL)(b+3);
         if ( !TrackPoints.Count() ) {
           ParceTrackSegmentInfo( &track, lt );
           track.Time.GetTT( &tTm );
           tmAdd = 0;
         }
       } else
       if ( *b == 0x8A ) {
         Log(( "EOT" ));
         if ( OnProgress ) OnProgress( this, 100 );
         break;
       } else {
         Log(( "UNK track segment" ));
         SetError( "Unknown data ID received: %02X", *b );
         return FALSE;
       }

       //Check points
       len -= sizeof(GH615_Track_Segment_LL);
       if ( (len%sizeof(GH615_TrackPoint_LL)) != 0 ) {
         SetError( "Invalid TRp structure" );
         return FALSE;
       }
       len /= sizeof(GH615_TrackPoint_LL);
       Log(( "Parce %d points (track info: %d..%d)", len, lt->from, lt->to ));

       //ParcePoints
       PGH615_TrackPoint_LL lp = (PGH615_TrackPoint_LL)( b + 3 + sizeof(GH615_Track_Segment_LL) );
       GH615_TrackPoint     p;

       for( n = 0; n < len; lp++, n++ ) {
         p.Lat       = RotateDWORD( lp->Lat ) / 1000000.;
         p.Lon       = RotateDWORD( lp->Lon ) / 1000000.;
         p.Alt       = RotateWORD( lp->Alt );
         p.Speed     = RotateWORD( lp->Speed ) / 100.;
         p.HeartRate = lp->HeartRate;
         p.Interval  = RotateWORD( lp->Interval );

         tmAdd += p.Interval;
         p.Time.Set( tTm + tmAdd/10 );

         TrackPoints.Add( p );
       }

       if ( OnProgress )
         if ( !OnProgress( this, TrackPoints.Count()*100/track.nPoints ) )
           break;

       if ( !ExecuteCmd( &cmd_GetNextTrackSegment, sizeof(cmd_GetNextTrackSegment), 0 ) )
         return FALSE;
     } //Read

 return TRUE;
}
/**********************************************************************
 GET WAYPOINTS
**********************************************************************/
#define GHD_GETWP_ID    0x77

#include <Global/pack1.h>
LOCALSTRUCT( GH615_Waypoint_LL )
  char  Name[ 7 ];
  BYTE  Type;
  WORD  Alt;
  DWORD Lat;
  DWORD Lon;
};
#include <Global/pop.h>

BOOL GH615::QueryWaypoints( void )
  {
#if 0
static BYTE rpl[] = {
  /*0000*/ 0x77,0x00,0x24,0x30, 0x31,0x34,0x20,0x20, 0x20,0x00,0x01,0x00, 0xFA,0x03,0x63,0x07,//w.$014   .....c.
  /*0010*/ 0x8D,0x03,0x9C,0xA1, 0x9B,0x53,0x54,0x41, 0x4C,0x20,0x20,0x00, 0x01,0x00,0xFA,0x03,//.....STAL  .....
  /*0020*/ 0x63,0x07,0x3B,0x03, 0x9C,0x99,0x87,0xA9                                           //c.;.....
};
     nReply = sizeof(rpl);
     MemMove( Reply, rpl, nReply );
#else
     GH615_SaveDevice _save(*this);
     if ( !_save ) return FALSE;

     if ( !ExecuteCmd( cmd_GetWaypoints, sizeof(cmd_GetWaypoints), 0 ) )
       return FALSE;
     if ( !ReadReplyTm(GHD_WPLIST_TMOUT) )
       return FALSE;
#endif

     Waypoints.DeleteAll();

     DWORD len;
     if ( (len=GH615_Checksum( ReplyData(), GHD_GETWP_ID, ReplyLen() )) == INFINITE )
       return FALSE;

     if ( (len%sizeof(GH615_Waypoint_LL)) != 0 ) {
       SetError( "Invalid WP structure" );
       return FALSE;
     }

     PGH615_Waypoint_LL lp = (PGH615_Waypoint_LL)(ReplyData()+3);

     for( len /= sizeof(GH615_Waypoint_LL); len > 0; len--, lp++ ) {
       GH615_Waypoint p;

       CopyText( p.Name, lp->Name, sizeof(lp->Name) );

       p.Type = (GH615Waypoints_t)lp->Type;
       p.Alt  = (double)RotateWORD( lp->Alt );
       p.Lat  = RotateDWORD( lp->Lat ) / 1000000.;
       p.Lon  = RotateDWORD( lp->Lon ) / 1000000.;

       Waypoints.Add( p );
     }

 return TRUE;
}

/**********************************************************************
 SEND WAYPOINTS
**********************************************************************/
int GH615::SetWaypoints( PGH615_Waypoint p, int cn )
  {  GH615_SaveDevice _save(*this);
     if ( !_save) return FALSE;

     if ( !p ) {
       p  = Waypoints.Items();
       cn = Waypoints.Count();
     }

     int nOk, n, wcn, rc;
     for( nOk = n = 0; n < cn; ) {
       wcn = Min( 10, cn );
       rc  = DoSetWaypoints( p, wcn );

       p  += wcn;
       n  += wcn;
       nOk  += rc;

       if ( OnProgress )
         if ( !OnProgress( this, n*100/cn ) )
           return nOk;
     }
     if ( OnProgress )
       OnProgress( this, 100 );

 return nOk;
}

#include <Global/pack1.h>
LOCALSTRUCT( qwp_LL )
  WORD id;
  BYTE len;
  BYTE cmd;
  WORD number;
};
LOCALSTRUCT( uwp_LL )
  BYTE id;
  WORD len;
  WORD count;
  WORD chs;
};
#include <Global/pop.h>

/*
  { "014",  (GH615Waypoints_t)1, 250, 56.821645, 60.596635 },
  { "STAL", (GH615Waypoints_t)1, 250, 56.821563, 60.594567 } };

  CMD:
    0000: 02 00 27 76 00 02 30 31 34 20 20 20 00 01 00 FA ..'v..014   ....
    0010: 03 63 07 8D 03 9C A1 9B 53 54 41 4C 20 20 00 01 .c......STAL  ..
    0020: 00 FA 03 63 07 3B 03 9C 99 87 DE                ...c.;.....
  RPL:
    0000: 76 00 02 00 02 00                               v.....
*/
int GH615::DoSetWaypoints( PGH615_Waypoint p, int cn )
  {
     if ( !p || !cn ) {
       FIO_SETERRORN( ERROR_INVALID_PARAMETER );
       return 0;
     }

     HDataPtr d;
     DWORD    len = sizeof(qwp_LL) + cn*sizeof(GH615_Waypoint_LL) + 1;
     Log(( "Allocate %d bytes for WP update", len ));
     if ( !d.Resize( len ) )
       return 0;

     qwp_LL *q = (qwp_LL*)d.Ptr();

     //Hdr
     q->id     = 0x0002;
     q->len    = cn * sizeof(GH615_Waypoint_LL) + 3;
     q->cmd    = 0x76;
     q->number = RotateWORD( cn );

     //Data
     PGH615_Waypoint_LL lp = (PGH615_Waypoint_LL)(((LPBYTE)q) + sizeof(*q));
     for( int n = 0; n < cn; lp++, p++, n++ ) {
       //Name
       int i;
       for( i = 0; i < sizeof(p->Name); i++ ) {
         if ( !p->Name[i] ) break;
         lp->Name[i] = p->Name[i];
       }
       for( ; i < sizeof(p->Name); i++ )
         lp->Name[i] = 0;
       lp->Name[6] = 0;

       //Fields
       lp->Type = p->Type;
       lp->Alt  = RotateWORD( ceil(p->Alt) );
       lp->Lat  = RotateDWORD( ceil(p->Lat*1000000.) );
       lp->Lon  = RotateDWORD( ceil(p->Lon*1000000.) );
     }

     //Checksum
     len = cn*sizeof(GH615_Waypoint_LL);
     ((LPBYTE)lp)[0] = GH615_CalcChecksum( 0, (LPBYTE)(&q->len), 4 + len );

     //Send command
     if ( !ExecuteCmd( d.Ptr(), sizeof(qwp_LL)+len+1, 0 ) )
       return 0;

     //Wait for reply
     if ( !ReadReplyTm( GHD_UPDATEWP_TMOUT ) )
       return 0;

     //Reply check
     if ( (len=GH615_Checksum( ReplyData(), 0x76, ReplyLen() )) != 2 )
       return 0;

     Puwp_LL qr = (Puwp_LL)ReplyData();
     qr->count = RotateWORD( qr->count );
     Log(( "Updated: %d wps", qr->count ));

     //Number of WP updated
 return qr->count;
}
